import numpy as np

from file_utils import FileUtils
from calibration import Calibration

if __name__ == '__main__':
    Xbe, Xco = FileUtils.read_base()
    Xcw = FileUtils.read_workpiece()
    tcb, rpy_cb, tbc, rpy_bc, teo = Calibration.base_calibrate(Xbe, Xco)

    print('基座相对于视觉的位置：')
    print(tcb)
    print('基座相对于视觉的姿态：')
    print(rpy_cb * 180.0 / np.pi)
    print('视觉相对于基座的位置：')
    print(tbc)
    print('视觉相对于基座的姿态：')
    print(rpy_bc * 180.0 / np.pi)
    print('靶球相对于法兰的位置')
    print(teo)

    tcw, rpy_cw = Calibration.workpiece_calibrate(Xcw)
    print('工件相对于视觉的位置')
    print(tcw)
    print('工件相对于视觉的姿态')
    print(rpy_cw * 180.0 / np.pi)

    tbw, rpy_bw = Calibration.calibrate(Xbe, Xco, Xcw)
    print('工件相对于基座的位置')
    print(tbw)
    print('工件相对于基座的姿态')
    print(rpy_bw * 180.0 / np.pi)
